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Abstract 

This paper contains a concise comparison of a number of nonlinear attitude filtering methods that 
have attracted attention in the robotics and aviation literature. With the help of previously published 
surveys and comparison studies, the vast literature on the subject is narrowed down to a small pool 
of competitive attitude filters. Amongst these filters is a second-order optimal minimum-energy filter 
recently proposed by the authors. Easily comparable discretized unit quaternion implementations of the 
selected filters are provided. We conduct a simulation study and compare the transient behaviour and 
asymptotic convergence of these filters in two scenarios with different initialization and measurement 
errors inspired by applications in unmanned aerial robotics and space flight. The second-order optimal 
minimum-energy filter is shown to have the best performance of all filters, including the industry standard 
multiplicative extended Kalman filter (MEKF). 

1 INTRODUCTION 

There are many highly-regarded attitude estimation methods that have been proposed in the literature [IJ 

[2]. In this paper we will focus on recursive, continuous-time filters and exclude non-recursive attitude 
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determination algorithms (e.g. [SHE]) > Hqo filters (e.g. [SOS]), discrete-update filters (e.g. El), and attitude 
filters that are based on dynamics or second-order kinematics (e.g. [3|H[|T2]). Within the class of algorithms 
that we do consider is the industry standard Multiplicative Extended Kalman Filter (MEKF |T3]). Recent 
competitors to the MEKF are the Right-Invariant Extended Kalman Filter (RIEKF) that is also known as 
the Generalized Multiplicative Extended Kalman Filter GMEKF na, the UnScented QUaternion Estimator 
(USQUE [IS]) as well as the Geometric Approximate Minimum-Energy (GAME) [T5j filter that is a second- 
order optimal minimum-energy attitude filter recently published by the authors. In addition, to these 
variable-gain filters we include the nonlinear complementary attitude observer e, that we refer to as 
the Constant Gain Observer (CGO). Due to the natural academic process of incremental development of 
algorithms, it can be difficult to determine what is the state-of-the-art version of any given algorithm. 
Moreover, different algorithms use different notation, and even different attitude representations, making 
cross comparison of competing algorithms difficult. As a result there is a lack of comparative studies in the 
literature that provide relative advantages and disadvantages of these methods compared against each other. 
Many of the advanced attitude filtering methods are still being advertised by demonstrating performance 
gain versus a naive implementation of the extended Kalman filter (EKF [1811191 1. an outdated attitude filter 
with well known convergence issues (cf. [20]). 

In this paper we document a comprehensive simulation study that sets out to compare the performance 
of state-of-the-art recursive attitude filters based on the continuous-time attitude kinematics model. The 
continuous-time selection criterion enables us to capture a larger pool of attitude filters for comparison study 
as opposed to discrete-time or continuous-discrete-time (discrete-update) attitude filtering that is employed 
in only a few filters, eg. EE- We are motivated by the goal of demonstrating the relative performance 
of the second-order optimal minimum-energy attitude filter recently published by the authors however, 
we have taken this opportunity to provide a consistent exposition of the structure, merits and performance 
of the different filters common in the literature. In particular, the paper presents 

• A literature survey that highlights the state-of-the-art recursive, continuous-time attitude filtering 
methods that are used in robotics applications. 

• Simple discretized unit quaternion implementations of the state-of-the-art attitude filters in consis- 
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tent language. MATLAB code for the simulation study is available from the fist author’s web page: 
https://sites.google.com/site/mohammadzamanishomepage. 

• A comprehensive Monte-Carlo simulation study comparing the state-of-the-art filters. Two scenarios 
are considered; the first derived from typical parameters associated with attitude estimation for appli¬ 
cations in unmanned aerial vehicles (UAVs), and the second from applications in the satellite attitude 
estimation problem. 

We also provide a discussion of how the different algorithms are tuned to obtain best performance. A key 
aspect of the simulation studies is the inclusion of gyroscope bias estimation in the estimator algorithms. 
In practice, high performance attitude estimation requires on-line bias estimation of the gyroscope, and we 
have also found that it is a critical factor when comparing relative performance of attitude filters. In fact, in 
the absence of the bias estimator most of the state-of-the-art algorithms have comparable performance. We 
will demonstrate this point using a simulation instance where no bias estimator is present in the competing 
filters. 

The remainder of the paper is organised as follows. Section [3] briefly explains attitude filtering and 
recapitulates the authors’ recent GAME filter, a second-order optimal minimum-energy attitude filter. In 
Section [2] a summary of some of the important attitude filtering methods is provided that also explains the 
choice of methods that are included in the simulation study. Section[4]describes the numerical implementation 
of filters considered in the simulation study. In particular, the discretization details of each method are 
provided separately. In Section [5] two comparison studies are considered, a UAV simulation setup and a 
spacecraft simulation setup for which the performance of the GAME filter is compared against the other 
attitude filters. Section [6] provides the conclusions of the paper. 

2 Attitude Filtering Methods 

This section includes a brief review of some the most important attitude filtering methods that are employed 
in aerial robotics. In particular, the main ideas behind some attitude filters are explained and a number of 
these methods are selected, against which the performance of the GAME filter is studied in simulations. 
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Attitude 

Filters 

state 

Ref. 

Compared Against 

Comments 

GAME 

SO (3) (Unit 

Quaternions in 

Simulations) 

Table 

ED 

MEKF, USQUE, Con¬ 
stant Gain Observer 

The GAME filter is a 2nd-order approximation to a minimum- 

energy filter derived directly on SO(3) that estimates the gyro bias 

quickly and is more robust to different noise levels with minimal 

tuning. 

MEKF 

Unit 

Quaternions 

El 

USQUE, SR-QCKF 

ED 

The MEKF estimates a unit quaternion by implicitly running an 

EKF in the vector space of its angular velocity input. 

RIEKF 

GMEKF 

Unit 

Quaternions 

m 

MEKF 

RIEKF is a right-invariant construction of the EKF, by consider¬ 
ing measurement noise modeled in the inertial frame. The RIEKF 

has better convergence properties than the MEKF. 

USQUE 

Unit 

Quaternions 

na 

MEKF, SR-QCKF gD, 

EKF US], BAF H 

A three-component attitude error is used to derive an unscented 

filter and the resulting estimated error is converted back to unit 

quaternions and multiplied with the previously estimated quater¬ 
nion to produce the filter’s estimate. 

BAF 

Unit 

Quaternions 

m 

USQUE 121 

The BAF achieves comparable performance to the USQUE, with 

the computational costs of particle filtering 

SR- 

QCKF 

Normalized 

Quaternions 

ED 

USQUE, MEKF |2D 

The USQUE requires more computation than the SR-QCKF but 

outperforms it in mean square error. 

AEKF 

Normalized 

Quaternions 

m 

MEKF 

AEKF is conceptually simpler than the MEKF, but with higher 

computational cost. The MEKF is also preferred as it avoids the 

embedding errors. 

CGO 

Unit 

Quaternions 

HD 


A carefully tuned constant gain is used with the same observer 

equations as in the MEKF or the GAME filter. It is very robust 

and asymptotically convergent with minimal computational load 

but requires exact tuning. 

EKF 

Normalized 

Quaternions 

m 

USQUE [ini 

The EKF in its standard form is outperformed by the USQUE. 

The AEKF and the MEKF build up on the EKF to improve the 

performance. 


Table 1: GAME filter: Geometric Approximate Minimum Energy filter, a second-order optimal minimum-energy filter, MEKF: 


Multiplicative Extended Kalman Filter, RIEKF: Right-Invariant Extended Kalman Filter, GMEKF: Generalized Multiplicative 
Extended Kalman Filter, SR-QCKF: Square-Root Quaternion Kalman Filter, USQUE: Unscented Quaternion Kalman Filter, 
BAF: Bootstrap Attitude Filter, AEKF: Additive Extended Kalman Filter, CGO: Constant Gain Observer, EKF:Extended 
Kalman Filter. 
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There are too many attitude filtering methods documented in the literature that we could hope to provide 
a detailed survey of all such methods in this paper. A good survey of early attitude filters is given in )24j 
while more recent material is provided in pj. The recent book [2] adds more current methods and detailed 
explanations, and we will try to cover more recent work on continuous-time recursive algorithms in the 
present paper. However, particular applications may require specific modifications or variations of filtering 
algorithms that, although important for that specific situation, are not interesting in the context of making 
a more general comparison. In this paper we will concentrate on our best understanding of what is the most 
generic algorithm for each filter architecture. 

Crassidis et. al [Ij concluded that “Many nonlinear filtering methods have been applied to the problem 
of spacecraft attitude determination in the past three decades. This paper has provided a survey of the 
methods that its authors consider to be most promising. It remains the case, however, that the extended 
Kalman filter, especially in the form known as the multiplicative extended Kalman filter, remains the method 
of choice for the great majority of applications.”. It remains the case that the MEKF m is an industry 
standard in recursive attitude filtering and it is an obvious benchmark for the comparisons undertaken in the 
present paper. The idea behind the MEKF is to consider the true attitude state as the product of a reference 
quaternion and an error quaternion that represents the difference between the reference attitude and the 
true attitude. The error quaternion is parameterized by a three dimensional representation of attitude and 
is estimated using an EKF. The MEKF estimates the true attitude by multiplying the estimated error 
quaternion (converted back to a unit quaternion) and the reference quaternion [1|. In order to avoid the 
redundancy of having to estimate both the reference quaternion and the error quaternion, the reference 
quaternion is chosen in a way that the error quaternion is the identity quaternion. Therefore, the MEKF 
directly calculates the reference quaternion as a unit quaternion estimate of the true attitude by implicitly 
running an EKF in the vector space of its angular velocity input. 

The standard EKF, derived in Euler angle local coordinates [19], is known to have poor performance 
and stability issues [20] and is outperformed by the MEKF m • The Additive Extended Kalman Filter 
(AEKF) |23j considers the unit quaternion representation of attitude, but initially ignores the unit norm 
constraint and has been shown to have no better performance than the MEKF [23]. There are many 
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other clever implementations of the the EKF addressing the attitude filtering problem. However, in our 
simulation study we only intend to consider straightforward implementations of the selected mainstream 
methodologies. A noteworthy family of attitude filters use a third-degree spherical-radial cubature integration 
rule to improve the numerical computation of Gaussian weighted integrals. A recent variant, the Square-Root 
Quaternion Cubature Kalman Filter (SR-QCKF) [2TI , offers improved numerical stability by guaranteeing 
the positiveness of the covariance matrix. 

The MEKF is in fact a special case of a more general filter design paradigm termed the left invariant 
extended Kalman filter IEKF [251125] , The invariant extended Kalman filter modifies the EKF equations by 
using an invariant output error rather than a linear error and also by updating the gain using an invariant 
state error instead of a linear state error. The right invariant EKF (RIEKF) or generalized MEKF (GMEKF) 
m is a closely related observer that uses the other-handed invariance for the filter derivation. The RIEKF 
is based on the assumption that the state and the output errors are configured in the inertial frame rather 
than the body-fixed frame, and although this assumption may in itself be questionable, it leads to better 
stability and conditioning of the associated Riccati equation. We include the GMEKF in the simulation 
study to verify its performance improvement over the equivalent IEKF and MEKF algorithms. 

The unscented quaternion estimator (USQUE [15]) is an attitude filter based on the unscented filter 
(UF [27] ) that has considerable support in the literature and has been proven to work well in many appli¬ 
cations. The UF uses a carefully chosen set of sigma points to approximate the probability distribution as 
opposed to the EKF that uses local Gaussian noise distributions. For a naive implementation of the UF, the 
updated quaternion mean would be obtained by an averaging process that would not in general maintain the 
unit norm condition of the unit quaternion representing the attitude. This is overcome in the USQUE [15] 
where a three-component attitude error is used to derive an unscented filter and the resulting estimated error 
is converted back to unit quaternions and multiplied with the previously estimated quaternion to produce 
the attitude estimate. The hope is that the singularities in the error representation will never occur since 
the quaternion error is small. In a recent paper m , it was shown that the USQUE had a similar estimation 
error compared to the MEKF although with a faster convergence rate. Due to its relatively strong following 
in the literature and the results in the recent paper [2T], the USQUE has been included in the simulation 
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study. Closely related to the USQUE filter is the Bootstrap Attitude Filter (BAF). This method is based 
on particle filtering where different to unscented filtering the samples or particles are drawn randomly to 
approximate the entire underlying distribution and not just the first two moments. The BAF has been 
shown |22| to achieve comparable results to the USQUE , albeit with the high computational load of particle 
filtering and hence is not selected for our simulation study. 

The second-order optimal minimum-energy (GAME) filter comprises the same observer equations as the 
continuous-time MEKF m , and the other invariant observers [25j[26]. However, the Riccati equation of 
the GAME filter includes curvature correction terms and a geometric second order derivative of the output 
function that are not present in the algorithms based on stochastic principles. These terms come from 
computing the full second-order information in the propagation equation for the taylor’s expansion of the 
value-function associated with a deterministic optimal filtering problem, rather than relying on the invariance 
to propagate local covariance estimates. 

There is a large class of constant-gain nonlinear observers designed for attitude estimation (cf. jT7 fl28fl34] 1 
that are also attractive methods to consider, as they are proven to produce asymptotically convergent 
estimates. A simple modification of the observer proposed in m allows the inclusion of a (constant) matrix 
gain rather than the scalar gains in the earlier papers. For applications where robustness and simplicity of 
an algorithm is critical, constant gain observers are of significant interest and it is natural to include the 
constant gain observer in the present simulation study. 

3 Attitude filter formulation 

Recently, the authors proposed the GAME filter a second-order optimal minimum-energy filter for 
the kinematics of the attitude of a rigid body. Here second-order optimality refers to using a second-order 
Taylor’s expansion approximation of the optimal value-function in the derivation of the filter. This section 
provides a concise summary of the structure of the GAME filter [16] as well as introducing the notation used 
in the paper. 
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The attitude kinematics of a rigid-body are given by 


X{t) =X(t)f2 x (t), X(0) = X o . 


(1) 


Here X is an S0(3)-valued state signal with the unknown initial value Xo and U gt 3 represents the angular 
velocity of the moving body expressed in the body-fixed frame. The lower index operator (,) x : R 3 —> so(3) 
denotes the skew-symmetric matrix 


o -n 3 

= n 3 o 

—H2 Hi 

A rate-gyro sensor measures the angular velocity; 


H2 
—Hi 
0 


( 2 ) 


u(t) = H(t) + Bnvn(t) + b(t). (3) 

The signals u £ R 3 and vq £ R 3 denote the body-fixed frame measured angular velocity and the input 
measurement error, respectively. The coefficient matrix Bq £ R 3x3 allows for different weightings for the 
components of the unknown input measurement error v. We assume that Bq is full rank and hence that 
Q n := BqBq is positive definite. In the case of the stochastic filters we will think, in a non-rigorous way, of 
the noise vq as a unit variance Gaussian process where the matrix Qq can be thought of as the covariance 
of the actual noise process. A rigourous development would introduce noise processes in the continuous¬ 
time model m, however, to simplify the development we will simply use a discrete noise process in the 
discretization model as is standard in the derivation of the stochastic invariant filters [T[|T4][25l[26] . For the 
minimum-energy deterministic filter, the signal vn does not have a stochastic interpretation, it is simply an 
auxiliary signal in the cost functional. For the constant gain observer design the noise signal is ignored. 

The signal b(t ) £ R 3 is an unknown slowly time-varying bias signal generated from 

b{t) = B b v b (t), 6(0) = 6 0 , (4) 

where B b £ R 3x3 is a full rank weighting matrix known from the model with Q b := B b BJ positive definite. 
The signal v b £ R 3 is a small unknown perturbation that is once again modeled as a stochastic process in 
the discretization of the system for the stochastic filters, an auxiliary signal for the minimum-energy filter 
and ignored for the observer design. The term bo £ R 3 is an unknown initial bias. 




Consider a collection of known direction vectors {iji} £ R 3 in the reference frame. Measuring these vectors 
in the body-fixed frame provides partial information about the attitude X. Typically, magnetometers, visual 
sensors, sun sensors and star trackers are deployed for this purpose. The following measurement model is 
used 

Vi(t) = X(t) T yi + DiWi(t), i = 1,- • • ,n (5) 

The measurements yi £ R 3 are measurements of the y* in the body-fixed frame and the signals wt £ R 3 are 
the unknown output measurement errors. The coefficient matrix Di £ R 3x3 allows for different weightings 
of the components of the output measurement error Wi . Again, assume that Di is full rank and Ri := DiDj 
is positive definite. In this case the noise u>i is straightforward to interpret as a unit variance Gaussian 
measurement noise. In the case of minimum energy filtering it is treated as an auxiliary signal despite its 
stochastic characteristic while for the observer design it is ignored. 

Consider the cost functional 

J(f; X 0 , 6 0 , vn |[o, t], U>|[o, t], M|[o, t]}) = ^ trace [(/ - X 0 )K^(I - A 0 ) T ] 



in which Kx 0 , Kb 0 € R 3x3 are symmetric positive definite matrices. In the case of minimum-energy filtering, 
the cost © can be thought of as a measure of the aggregate energy stored in the unknown initialisation and 
measurement signals of ©, ©, © and ©. In the case of the stochastic filters this cost functional would 
be related to an expected value of a log-likelihood cost functional, although, the derivations of the stochastic 
attitude filters considered mmmm do not work from first principles. 

In practice, the most common representation used for implementation of attitude estimation algorithms 
is the unit-quaternion representation. Unit quaternions lead to more efficient and more robust numerical 
implementations of the algorithms due to the simple renormalization operation to preserve the representation 
constraint. Moreover, unit quaternions do not have the singularity issue that is associated with many other 
rotation representations. Unit quaternions suffer from non-uniqueness of the representation, however, this 
issue has been well discussed in the literature and does not pose practical issues for a careful implementa¬ 
tion [2]. Finally, algorithms such as the MEKF or the USQUE require the unit quaternion representation. 
For these reasons, we will use the unit quaternion representation to express and compare all the algorithms 
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considered. Details on the unit quaternions, using the notation in this paper, can be found in the appendix 


of Mahony et. al m 

The attitude kinematics X = X£l Xl in the unit quaternions form is 

9 = \ A (tyQ, 


where SlgR 3 and 


The vectorial measurements yi 


A ( 7 ) : = 



n -n x 

X T yi + DiWi , can be written as 
Vi = q^ 1 ® p (yi) ® q + DiWi, 


(7) 


( 8 ) 


(9) 


in quaternion notation. Here p(f2) = (0, fl) T with inverse projection pi such that pf(p(fl)) = Cl. 

The basic structure of the continuous-time estimation algorithms considered is 

9 = ^ A (u — b — P a A^j q, (10a) 

where b is the estimate of the bias b given from 

b = PjA , (10b) 

with initial conditions &(0) = 0, q( 0) = 1. The innovation term A is defined as 


A :== “2/*)) x Vi- 

i 


(10c) 


where we recall that Ri := DiDj. The gains P a and P c are symmetric 3x3 matrices updated from a Riccati 
like equation. For example, in the case of the GAME filter m, and recalling Qq := BB T , Q b '■= B b Bj, the 
Riccati equation in continuous time is written m 

P a = Qn + 2P s (P a (2(u - 5) - P 0 A) X ) + P a (E - S)P a - Pj - P c , 

(lOd) 


P c = -(u-b- P a A) x P c + P a (E - S)P C - P b: 


where the gain P b is given from 

P h = Qb + P C (E - S)P C , 


(lOe) 
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with 


-B := trace(C)/ — C, (11) 

i i 

Here P a (0) = ( tTa,ce(Kx 0 )I — Kx 0 )~ l where Kx 0 is a known variable from the cost function. The vector 
y = is analogous to to 0 and the operator P s is a symmetric projector defined in equation (fill) 

below. 

The matrix 


P = 


( 


Pa 

Pb 

Pb 

Pc 


\ 


contains the full gain matrix information including both the attitude and bias states. It is an interesting 
feature of all the attitude filtering algorithms that the matrix Pf, is not required to implement the filter 
update equations (llOal) and (HObl) . It is of course, coupled in the differential Riccati equation (llOdl) and 
(II Pel) and must be computed. However, it is natural to write the Riccati equation in decoupled form with 
separate matrices P a , Pb, P c . Although equations m are specific to the GAME filter, the same notation 
and structure has been used for all the filters in this paper. 

In order to numerically compare the filters, discrete-time implementations of the continuous-time filters 
are required. This is not a trivial task in attitude filtering (and similar problems) as the Lie group config¬ 
uration of the underlying state space has to be preserved during the numerical computation. In practice, 
at least when the time step considered is small compared to the motion of the vehicle, the usual approach 
taken is to use a simple Lie group Euler numerical integration. Assuming a small time step dt , then Q is 
approximately constant on the time period [kdt, (k + 1 )dt\ for k £ N. Denote this value by Then the 
exact integration of 0 yields 

( 12 ) 


Qk +1 = -exp(dtA(Q k ))q. 


This numerical integrator is used for the attitude state (II Oal) . The bias observer (11 Obi) as well as the Riccati 
equations (llOdl) and (IIPel) live in linear vector spaces and are implemented using a classical Euler iteration. 
The same simple numerical integration scheme is used for all the filters in this paper since the focus is on a 
comparison of the performance of the different filter architectures and not on the numerical implementation. 
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4 Discretized Implementations of the Attitude Filters Selected for 


the Simulation Study 

In this section unit quaternion discretized implementations of the attitude kinematics © and the filters 
selected for our simulation study are provided in Tables [2] to [7] below. The following notation is instrumental 
in these formulations. 

Recall that the set of skew symmetric matrices is denoted by so(3) and is the Lie-algebra of SO(3). The 
operation (-) x is given by © and provides the isomorphism between R 3 and so(3). The inverse operator 
vex : so(3) —R 3 extracts the skew coordinates, vex(fl x ) = 17. 

Denote a symmetric positive semi-definite matrix by B > 0 (a symmetric positive definite matrix is 
denoted by B > 0). The seminorm ||.||ft : R 3x3 —> R(j" is given by 

\\M\\ r := y^trace(Mi?M T ), (13) 

where R £ R 3x3 > 0. Note that if R > 0 then ||M||/j coincides with the Frobenius norm of MR 1 / 2 . The 
symmetric projector P s is defined by 

P S (M) := 1/2(M + M t ). (14) 

The skew-symmetric projector P a is defined by 

P a (M) := 1/2 (M - M t ). (15) 

Note that for every A G so(3), M £ R 3x3 and S = S T £ R 3x3 , 

trace(^4P s (M)) = 0, trace(P s (5'A)) = 0. (16) 


12 



q{k + 1) = | exp (dtA(n(k)))q(k), q{ 0) = q 0 , 

Kinematics 

0 -n(k) T 

A(fl(k)) = 

n(k) 


u(k) = fl(k) + b(k) + BqVq( k), 

Gyro Measurements 

Qn = BnB' n 


Bias Model b(k + 1) = b(k) + dt[BbVb(k)}, Qb = B V B' V 


Di(k) = p t (g(fc ) _1 0 P (ili) 0 q(k)) + DiWi(k), 
Vector Measurements Ri = DiD’ i: 

0 

P (Vi)= , p t (p(yi)) = in 

Vi 


Table 2: Discrete Attitude Kinematics and Measurements 
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q(k + 1) = ^ exp (dtA[u(k) - b(k) + P a (k)A(k)])q(k), 

A ( k ) = Y^Vi( k ) x (K\Uk) - Vi(k ))), 

i 

Attitude Observer 

Vi(k) = P t (g(fc ) _1 ® p (in) 0 g(fc)), q(0) = [1 0 0 0] T 

Bias Observer b(k + 1) = b(k) + dt[P c (k) T A(fc)], S(0) = [0 0 0] T , 

P a {k + 1) = P a {k) + dt[Qn + 2P s [P a (k)(u(k) - b(k) - 

iP 0 (fc)A(fc))x - Pc(*)] + P a (k){E{k) - S(k))P a (k)], 

P c {k + 1) = P c (k) + dt[-(u(k) - b(k) ~ \P a {k)A{k)) x P c {k) + 
P a (k)(E(k) - S(k))P c (k) - P & (fc)], 

Riccati Gains 

P b (k + 1) = P b {k) + dt[Q b + P c (k)(E(k) - S(k))P c (k)}, 

S ( k ) = E,:(^( fc ))J^ rl (yi(fc))x, 

E(k) = trac e(C(k))I - C(k), 

C{k) = EiP siR-^m) - yi{k))y{k)J ), 

Table 3: GAME Filter 
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q(k + 1) = ^ exp (dtA[u(k) - b(k) + P a (k)A(k)])q(k), 

A ( fc ) = x (KHHk) ~yi(k))), 

i 

Attitude Observer 

ili{k) = P f (g(fc) _1 <g> p (yi) ® g(fc)), q( 0) = [1 0 0 0] T 

Bias Observer b(k + 1) = b(k ) + dt[P c (fc) T A(fc)], 6(0) = [0 0 0] T , 

P a (fc + 1) = P a (fc) + dt[Qn + 2P S [P a (k)(u(k) - b(k)) x - P c (k)\ - 
P a (k)S(k)P a (k)], 

Pc(k + 1) = P c (k) + dt[-(u(k)-b{k)) x P c (k)-P a (k)S{k)P c (k) - 
Riccati Gains Pb(k)], 

Pb{k + 1) = Pb(fc) + dt[Q b - P c (k)S(k)P c (k)], 

s (k) = Ei(yi( fc ))I i? r l (yi( fc ))x, 

Table 4: MEKF 
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q(k + 1) = — ex.p(dtA[u(k) — b(k) + P a (k) A(k)])q(k), 

Attitude Observer 

A ( fc ) = ^2 in x ( R z _1 (& - &(*)))> 

i 


ih{k) = p\q(k) ® p(y*(fc)) <S>g _1 (fc)), g(0) = [1 0 0 0] T 

Bias Observer 

6(fc+l) = 6(fc)+dt[g(/c)^ 1 i8)p(E , c (A:) T A(/c))(g)g(fc)], 6(0) = [0 0 0] T , 


P a (/c + 1) = P 0 (fc) + cft[Qn ^ 2P s [P c (fc)] - P a (fc)5'(fc)P a (A;)], 

Riccati Gains 

P c (fc + 1) = P c (k) + dt[-P c (k)q(k)<S>p{{u(k) - b(k)) x )^q~ 1 (k) - 

P a (k)S(k)P c (k) - Pb(k)], 

Pb(k+ 1) = P b (k)+dt[2P s (q(k)®p((u(k)-b(k)) x )®q~ 1 (k)P b (k) + 

Q b — P c (k)S(k)P c {k)], 


S(k) = Y.i(!>i(k)Yx R i'H!)i(M))xz 


Table 5: RIEKF (GMEKF) 
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Table 6: USQUE 
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q{k + 1) = — exp (dtA[u(k) — b{k) + kpA(k)])q(k), 


A(k) = ^2vi( k ) X Vi ( k )- 

Attitude Observer 



m{k) = P t (g(fc)” 1 ® P (yi) <s> <K fc ))> 9(0) = [1 0 0 0] T 

Bias Observer 

b(k + 1) = b(k) - dt[k!A(k)], 6(0) = [0 0 0] T 


Table 7: CGO 
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5 Comparison Study 


In this section, multiple simulated experiments are presented that compare the filtering methods considered 
in Section [2] 


5.1 Case 1: Measurement Errors Expected from Low-Cost UAV Sensors 

The first experiment considered simulates attitude estimation for a low cost unmanned aerial vehicle (UAV) 
system for which the measurement errors are relatively large. It is also assumed that the rotation and the 
bias initialization errors are large, as is the case when using low cost MEMS gyros such as the popular 
InvenSense MPU-3000 family. The simulation parameters are summarized in Table [HJ The GAME filter 
(Tabled) is compared against the MEKF (Table 0|), the RIEKF (Table [5]), the USQUE (Table [6]) and the 
CGO (Table [3 that are explained in detail in Section [2j 

Simulated attitude kinematics and measurements (see Table [2]) are considered with the following parame¬ 
ters that are also summarized in Tabled] A sinusoidal input U = [sin(||t) —sin(|^t + 7r/20) cos(|yf)] drives 
the true trajectory q. The input measurement errors v and Vb are Gaussian zero mean random processes 
with unit variance. The coefficient matrix B is chosen so that the signal Bv has a standard deviation of 
25 degrees per ‘second’. The bias variation is adjusted by Bb such that BbVb has a standard deviation of 
0.1 degrees per ‘second’ squared. The system is initialized with a unit quaternion representing a rotation 
with standard deviation of std qo = 60 degrees and an initial bias with standard deviation of stdb 0 = 20 
degrees per ‘second’. We assume that two orthogonal unit reference vectors are available. We also consider 
Gaussian zero mean measurement noise signals Wi with unit standard deviations. The coefficient matrices 
Di are chosen so that the signals DiWi have standard deviations of 30 degrees. Although the two filters do 
not have access to the noise signals vq, Vb and Wi themselves, they have access to the matrices Qn = BB 1 , 
Qb = BbBj and Ri = D^Dj. The filters are simulated using zero initial bias estimates and using the identity 
unit quaternion as their initial quaternion estimate. 

The following filter initializations are considered that are also summarised in Table [9] The initial 
quaternion and bias gain matrices of the USQUE are chosen according to the variance of the system’s 
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initial quaternion in radians P Q (0) = std 2 0 / 3 X 3 and the variance of the system’s initial bias in radians 
per ‘seconds’ P&(0) = std^I^xs- The initial quaternion and bias gain matrices of the GAME filter, the 
MEKF and the RIEKF are chosen according to the inverse variance of the system’s initial quaternion in 
radians P a ( 0 ) = - 132 -/ 3 x 3 and the inverse variance of the system’s initial bias in radians per ‘seconds’ 
Pb( 0) = - 733 -/ 3 x 3 as these filters are in the information form. The coupling initial gain is considered as the 

ab o 

zero matrix P c (0) = 63 x 3 for all the filters. The CGO is initialized with k p = 1 and kj = 0.3 as in [ITj. 


Time Step 

0.001 (s) 

Simulation Time 

50 (s) 

Angle of Rotation Initialization 

AT ~ (0, 6 O 2 ) 0 

Error 


Bias Initialization Error 

( 0 , 20 2 )3 

Reference Directions 

Vi = [1 0 0 ], 02 = [0 1 0 ] 

Input signal 

n = [sin(ff t) -sin(fff+ 71-/20) cos(fff)]^ 

Input error Bqvq 

A/-~(0,25 2 )3 

Bias Variation Bf,Vb 

Af ~ (0,0.l 2 )^- 

Measurement error DiWi 

AT ~ (0,30 2 ) 0 


Table 8 : Simulation Parameters for the UAV Situation Case 1 


Figures [1] and [3] show the performance of the GAME filter compared against the MEKF, the RIEKF, 
the USQUE and the CGO in the Case 1 experiment. We have performed a Monte-Carlo simulation and the 
RMS of the estimation errors of the two filters are shown for 100 repeats. 
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USQUE 

Pa( o) = std 2 Q I 3x 3 , P b ( 0) = stdl o I 3x3 , P c ( 0) = 0 3x3 

GAME, MEKF, RIEKF 

Pa{ 0) = -^p~I 3x3 , Pb( 0) -^p~I 3 x 3, Pc(0) = 0 3x3 

10 bo 

CGO 

k p = 1, ki = 0.3 


Table 9: Initial Filter Gain Matrices for the UAV Situation Case 1 


Figures[l] and the zoomed version (Figure[2]) indicate that the RMS of the rotation angle estimation error 
of the proposed GAME filter rapidly converges towards zero in the transient period and also maintains the 
lowest error compared to the rest of the filters in the asymptotic response. Figure [3] shows that the GAME 
filter also has the lowest asymptotic bias estimation error compared to the bias estimation error of all the 
other filters. 

Note that the initial peak in the angle error of the GAME filter, the MEKF and the RIEKF is associated 
with the period where the bias estimates of these filters are not accurate enough yet. The adaptive nature 
of these filters is allowing a higher uncertainty in the angle estimates until a reasonable bias estimate is 
obtained which is then used to achieve an accurate asymptotic angle estimate. The bias error of these filters 
in Figure [3] is showing the peaking phenomenon that is also seen in a high-gain observer. This is not the 
case for the USQUE which has the fastest angle estimation but the slowest bias estimation. This is due to 
the fact that the USQUE is setting a high gain for its angle observer and a low gain for its bias observer 
that leads to the amplified noise on the angle estimation error of the USQUE in Figure [1] The CGO on the 
other hand is not an adaptive filter but it has an asymptotically convergent estimation error that is a priori 
adjusted through the gains k p and kj. 

A key aspect of the comparison is the bias estimation part and its effect on the angle estimation perfor¬ 
mance of the filters. While bias estimation is a practical requirement, it was observed that excluding the bias 
and applying proper tuning to the filters leads to little difference in the transient and asymptotic behaviour 
of the filters. Figure [4] demonstrates an instance of the previous simulation where no bias is considered. It 
was easily possible to find tuning parameters that yield almost identical transient and asymptotic behaviours 
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of all the competing filters except the constant gain observer (CGO). The CGO in this case has to trade-off 
between the fast transient behaviour and the asymptotic estimation error. A higher gain results in the faster 
transient convergence, however, with a larger asymptotic estimation error. This problem is not present in 
the other filters as their gains are adjusted dynamically by the filter. 

Also note that the RIEKF is in fact outperforming the MEKF as was noted in m too. It is interesting 
that the CGO has the second lowest estimation error with the lowest computational cost. Of course the 
downside of the CGO is that it needs exact tuning depending on the information about the true attitude 
trajectory that might not always be available a priori. The USQUE has the fastest angle convergence to a 
relatively low error. However, the noisy asymptotic performance (which might be due to lack of complicated 
tuning in our experiments), very slow bias estimation and the heavy computational cost of the USQUE 
compared to the other filters considered makes the USQUE not desirable for the UAV application considered. 
This argument is further investigated in § 15.31 below. 

5.2 Case 2: Measurement Errors Expected in a Satellite 

In this experiment much smaller measurement error signals are considered as is the case for a satellite 
attitude filtering problem. The simulation parameters are according to the reference |35j and are described 
in Table flOl Note that the angular velocity input of the attitude kinematics has a much smaller frequency 
compared to the UAV case as the movement of a satellite is restricted to an earth orbit. 

The initial gains of the filters are chosen according to Table [TT] Note that these values are not exactly 
according to the statistics of the initialization errors of the system, as was the case in our previous experiment. 
This is to avoid singularities that are due to the fact that the numerical values of some simulation parameters 
are too small and close to the computational limits of the MATLAB programming platform. 

Figures [5] and [G] show the performance of the GAME filter compared against the MEKF, the RIEKF, the 
USQUE and the CGO in the Case 2 experiment. Note that the figures shown are due to a single repeat of 
an experiment that is typical for the results seen in more repeats. In this case the small frequency of the 
angular velocity input leads to a slow dynamics of the angle trajectory. Due to this slow dynamics and also 
due to the small measurement errors considered, the estimation errors of all the filters converge towards zero 
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Figure 1: Case 1: The RMS of the estimation error in angle of rotations for a UAV simulation setup. Note 


that the angle axis is in logarithmic scale. 
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Figure 2: Case 1: The RMS of the estimation error in angle of rotations for a UAV simulation setup. 
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Zoomed on the asymptotic error of the GAME filter, the MEKF and the RIEKF. Note that the angle axis 


is in logarithmic scale. 
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RMS of Bias Estimation Errors 



Figure 3: Case 1: The RMS of the bias estimation error for a UAV simulation setup. Note that the bias axis 
is in logarithmic scale. 



Time (s) 


Figure 4: Case 1: The estimation error in angle of rotations for a UAV simulation setup with no bias. Note 
that in this case it is easily possible to obtain similar convergence behaviour among the filters. 
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Time Step 


0.001 (s) 


Simulation Time 

50 (s) 

Angle of Rotation Initialization 

Af ~ (0,60 2 ) 0 

Error 


Bias Initialization Error 

Af ~ (0,20 2 )^ 

Reference Directions 

Vi = [1 o 0], y 2 = [0 1 0] 

Input signal 

D = sin(^t)[ 1 -11}°- 

Input error Bnvn 

Af ~ (0,0.31623 2 ) M 

Bias Variation B^Vb 

Af ~ (0,0.031623 2 )" rad 

Measurement error DiWi 

AT ~ (0,1 2 )° 


Table 10: Simulation parameters for a satellite situation 


rapidly. The GAME filter outperforms the other filters in achieving the lowest asymptotic estimation error. 
The USQUE converges very fast although its asymptotic estimation error is noisy as was the case in the 
UAV experiment. 

5.3 Gain Scaling 

One can choose different gain scalings for a particular filter. A higher scaled gain can result in faster 
convergence of a filter with the disadvantage of larger asymptotic estimation error. Depending on the 
application, one has to trade-off between the transient and the asymptotic performance of a filter. The 
following two examples are demonstrations of this trade-off seen in the results above. 
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USQUEy 
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Figure 5: Case 2: The estimation error in angle of rotations for a satellite simulation setup. Note that the 
angle axis is in logarithmic scale. 


Bias Estimation Errors 



Figure 6: Case 2: The bias estimation error for a satellite simulation setup. Note that the bias axis is in 
logarithmic scale. 
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USQUE 

Pa( o) = std 2 Q I 3x 3, P b ( 0) = stdl o I 3x3 , P c ( 0) = 0 3x3 

GAME, MEKF, RIEKF 

Pai 0) = ^^3x3, Pb( 0) = ^/ 3 X3, Pci 0) = 0 3x3 

90 bo 

CGO 

k p = 10, ki = 2 


Table 11: Initial Filter Gain Matrices for the Satellite Case 2 


As was apparent in Figures [H and 0 the USQUE is inherently using a higher angle gain than the other 
filters. In fact, zooming into the asymptotic angle estimation error graph of the USQUE, it is apparent 
that the estimation error is approximately 30 times lager than the estimation error of the other filters (See 
Figure 0. Consider using a scale factor of 30 multiplying gain P a of the GAME filter. As can be seen 
in Figure 0 the estimation errors of the two filters are now almost identical, confirming that the USQUE 
algorithm is rendering an undesirable scaling in its angle gain P a that, although it results in a very fast angle 
estimation also results in a noisy asymptotic estimation error. We have tried to account for this effect in 
the USQUE algorithm by means of simple tuning. However, due to the involved nature of this algorithm 
(Table 0) it is unclear how to compensate for the clear scaling issue in the gain tuning - this is a clear 
disadvantage of the USQUE algorithm. It is worth noting, that the bias estimation of the GAME filter is 
still much faster than that of the USQUE indicating the advantage of the GAME filter over the USQUE 
even in this case. 

The authors believe that the original published formulation of the RIEKF m has two typographic errors 
- details are provided in the Appendix section. As a result, the originally published RIEKF has a larger gain 
than the RIEKF considered here (Table 0. This is investigated in the following simulation comparing the two 
formulations of the RIEKF. As can be seen in Figure^ the original RIEKF has a faster decaying convergence 
error. However the asymptotic error of the original RIEKF is approximately two times more noisy than the 
alternative formulation, confirming the gain difference of the two filters. This point is important since the 
relative difference in gain scaling is of the same order as the performance advantage of the RIEKF over the 
MEKF. 
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Figure 7: Case 1: The USQUE is compared against the GAME filter when the angle gain of the GAME P a 
is multiplied by 30. Note that the angle axis is in logarithmic scale. 
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Figure 8: Case 1: The USQUE is compared against the GAME filter when the angle gain of the GAME P a 
is multiplied by 30. Note that the bias axis is in logarithmic scale. 
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Figure 9: Case 1: The gain difference between the RIEKF in l 14j and the RIEKF (TableO. Note that the 
angle axis is in logarithmic scale. 


29 








6 Conclusions 


In conclusion, the second-order optimal minimum-energy (or the Geometric Approximate Minimum-Energy, 
GAME) filter proves to be highly robust both in situations with large measurement errors and fast attitude 
dynamics, such as the case of a low cost UAV, and also in a situation with small measurement errors and slow 
attitude dynamics such as in the case of a satellite. In fact in both cases, it was shown in the previous section 
that the GAME filter outperforms the state-of-the-art attitude filters such as the Multiplicative Extended 
Kalman Filter MEKF, the Right-Invariant Extended Kalman Filter RIEKF, the UnScented QUaternion 
Estimator USQUE and the Constant Gain Observer CGO. 

The difference between the MEKF and RIEKF is subtle. In low noise situations the two algorithms are 
essentially equivalent as can be seen from the satellite simulation. In the case of high noise it is clear that 
the more stable noise representation associated with the RIEKF leads to better conditioning of the gain and 
overall improved performance. 

The USQUE as published is very fast in angle estimation but slow in bias estimation. Moreover, the 
asymptotic angle estimation error of the USQUE is very noisy due the high gain of the angle error. It is clear 
that the tuning for the USQUE is not optimal, however, due to the complexity of the algorithm it is difficult 
to see how to improve the gain tuning. Given the relative performance of the GAME, MEKF and RIEKF, 
along with the computational cost of the USQUE, it is unlikely that the effort required to tune the USQUE 
would be worth pursuing. A key observation from this study, however, is that comparison of attitude filters 
must consider both the transient and the asymptotic behaviours and that any filter can produce improved 
transient behaviour if the filter gain is increased. 

The CGO yields desirable low estimation errors with minimal computational cost. However, the gains of 
the CGO need to be tuned a priori and the performance obtained here was heavily based on having used 
the GAME filter to obtain optimal asymptotic gains for the CGO. There is a clear penalty in transient 
convergence for the CGO compared to the variable gain filters, demonstrating the fundamental advantage 
of optimal filtering. Against this, the constant gain observer does not pose any risk on having an unstable 
Riccati equation and the computational complexity is very low. The CGO clearly remains a very viable filter 
option in a range of important applications. 
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Appendix: RIEKF 


Note that the formulation provided here for the RIEKF is different to the one given in the reference 0 in 

two aspects. First, the state error in m is modeled two times larger, giving the filter formulation a factor 

of 2 higher gain. Secondly, there seems to be a factor of | inconsistency in the A matrix calculation of the 

RIEKF pTj that leads to occasional singularities in the simulation results of the RIEKF. The authors believe 

that both these differences are minor typographical errors in the original paper j!4l . We provide an updated 

derivation of RIEKF in the following discussion. 

The RIEKF formulation considers the quaternion system model 

' 

g = -g0f2, 

u = n - 2g _1 0 (Buvn) ®q + b, 

< _ (17) 

b = q- 1 <3 (B B v b ) 0 q, 

Vi = g _1 0 (yi + DiWi) 0 q. 

Note that the state and the output errors are modelled in the inertial frame which is different to the 
conventional modelling of errors in the body-fixed frame. The RIEKF then is 

q= -q®(u-b + 2 g" 1 0 (J2, K q (yi - yi) 0 q), 

' Vi = ?0 {yi) <8>g _1 , ( 18 ) 

b = g -1 0 Kb(Vi - yi) ® g- 


Consider the errors 

! g = g 0 g _1 , 

6 = g 0 (6 — &) 0 q~ 1 . 

The error system is given by 


(19) 


q = ~\q 0 (b) + (J2i Kqiiji -yi)®q)®q~q® (Bqvq), 

* b = +2(B n vn) x b + q~ l 0 (£V K b (in -yi)®q~ B B v b + (g _1 0 (u) 0 g) xb, ( 20 ) 

ft = g _1 0 (u — 6) 0 g. 

Next, linearize the error system using q — > [1, ^<5g] T and b —> Sb , and neglect the quadratic terms in noise 
and infinitesimal state error similar to [14]. Note that the factor of ^ in the linearized quaternion is not 
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considered in the original paper m. 




\ u j 


= (A - KC) 


( Sq ^ 


\ Sb J 


Bqvq + {J2i KqDiWi) 
y B b v b + QY K b DiWi) I 


( 21 ) 


where 




C = (2(fc) x 0), K 


-[K q ,K b ] T . 


y 0 U X y 

Then similar to the EKF the full filter is realized using 

K=PC T R~\ 

< p = AP + PA T +Q - PC T R^CP, 


( 22 ) 


(23) 


Q = diag(Q^, Q b ). 

Note that in [14] there is a typographic error in the matrix A with an extra factor of ^ multiplying the 
identity matrix I. 

If the state noise is considered without a factor of two then 


q= 

u = fl - g” 1 <g> ( Bqvq ) ®q + b, 

< _ (24) 

b = q~ l (g> ( B B v b ) <g> q, 

Vi = <? _1 ® {yi + DiWi) 8 > q , 

and the matrix C of the RIEKF is modified to 


c = m x o), 


(25) 


the version used in Table [5] 
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